#ifndef SP_BMS_H__
#define SP_BMS_H__

#include <ros/ros.h>
#include <ros/ros.h>
#include <ecl/threads.hpp>
#include"geometry_msgs/Twist.h" 
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/Point32.h>
#include <geometry_msgs/Pose.h>

#include <sensor_msgs/Imu.h>
#include <string.h>
#include "std_msgs/String.h"
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <nav_msgs/Odometry.h>
#include <tf/tf.h>
#include <std_msgs/Float64.h>
#include <sensor_msgs/Joy.h>
#include "sensor_msgs/LaserScan.h"
#include "geometry_msgs/PoseWithCovarianceStamped.h"
#include "geometry_msgs/PoseArray.h"
#include "geometry_msgs/Pose.h"
#include "geometry_msgs/PoseStamped.h"
#include "nav_msgs/GetMap.h"
#include "nav_msgs/SetMap.h"
#include "std_srvs/Empty.h"
#include "geometry_msgs/Pose2D.h"

// For transform support
#include "tf2/LinearMath/Transform.h"
#include "tf2/convert.h"
#include "tf2/utils.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#include "tf2_ros/buffer.h"
#include "tf2_ros/message_filter.h"
#include "tf2_ros/transform_broadcaster.h"
#include "tf2_ros/transform_listener.h"
#include "message_filters/subscriber.h"

#include <mutex>
#include <shared_mutex>
#include <sys/time.h>
#include <time.h>
#include <vector>
#include <queue>
#include <cmath>
#include <map>

struct pose_type {
	double x;  // Ä¬ÈÏµ¥Î»Îªm£¨»òm/s£©
	double y;  // Ä¬ÈÏµ¥Î»Îªm£¨»òm/s£©
	double theta; // Ä¬ÈÏµ¥Î»Îªrad£¨»òrad/s£©
};



class Tfpub
{
public:
    Tfpub();
    ~Tfpub();
    bool init();
    void destroy();
    bool update();
    void loop();
    void thread_func();
    void timerCallback(const ros::TimerEvent& e);
    void workCallback(const geometry_msgs::PoseWithCovarianceStamped& p);


private:
    std::map<double, pose_type> laser_odom_pose;//存储里程计位姿数据，队列中存储10s的数据大小

    std::shared_timed_mutex mtx_laser_odom_pose;

    pose_type odom_pose = {0.0,0.0,0.0};

    bool first_in_laser_odom = true;

    ros::Subscriber laser_scan_sub_;
    pose_type init_odo_pose;
   
    ros::Publisher odom_pub;
    int times_;
    ros::Timer timer1;
    pose_type laser_pose_;
    pose_type last_laser_pose_;
    ros::Publisher pub_basepose;
    geometry_msgs::Pose2D base_pose_;

    double x_;
    double y_;
    double z_;
    double theta_;

    
    ros::NodeHandle nh_;
    std::string port_name_;
    int baudrate_;
    int loop_rate_;
    std::string cmd_;
    bool is_connected_;
    int flag_init;
    unsigned short encoder_[4];
    ros::Time last_time_;
    unsigned short time_stamp_;
    ecl::Thread thread_;
	ros::Subscriber laser_sub;
	ros::Publisher pub_battery;
	std_msgs::String msg;
    geometry_msgs::PoseWithCovarianceStamped last_published_pose;
    tf2::Transform latest_tf_;
    boost::mutex mutex_;
    bool tf_broadcast_;
    std::shared_ptr<tf2_ros::TransformBroadcaster> tfb_;
    std::shared_ptr<tf2_ros::TransformListener> tfl_;
    std::shared_ptr<tf2_ros::Buffer> tf_;
  
    tf2_ros::TransformBroadcaster odom_broadcaster;
  
};

#endif
